#include "Factory.h"
#include <algorithm>
#include "Vector2f_Math_Functions.h"
#include "InputManager.h"

bool wayToSort(Object* a, Object* b) { return a->pos.x < b->pos.x; }
float maxCollisionDistance = 200.0f;


// static variables need to be explicitly added to one CPP file
vector<Object*> Factory::m_objectList;


void Factory::DeleteObject(Object *o)
{
	for (int i = 0; i < m_objectList.size(); ++i)
	{
		if (m_objectList[i] == o)
		{
			// We've found it!  Remove from list and delete:
			m_objectList[i] = NULL;
			delete o;
			return;
		}
	}

	// Clean up empty items from our list:
	m_objectList.erase(
		std::remove(m_objectList.begin(), m_objectList.end(), (Object*)NULL),
		m_objectList.end()
		);

}

void Factory :: DeleteAll()
{
	for (int i = 0; i < m_objectList.size(); ++i)
	{
			delete m_objectList[i];
			m_objectList[i] = NULL;
	}

	// Clean up empty items from our list:
	m_objectList.erase(
		std::remove(m_objectList.begin(), m_objectList.end(), (Object*)NULL),
		m_objectList.end()
		);
}

void Factory::AddObject(Object *o)
{
	m_objectList.push_back(o);
}

void Factory::RenderAll(sf::RenderWindow* window)
{
	for (int i = 0; i < m_objectList.size(); ++i)
	{
		if (m_objectList[i]->isVisable)
			m_objectList[i]->Render(window);
	}
}

void Factory::UpdateAll(float deltaT)
{
	
	
	//run all objects individual update function (for input and fucntionality etc)
	for (int i = 0; i < m_objectList.size(); ++i)
	{
		m_objectList[i]->Update(deltaT);
	} 

	//Physics
	for (int i = 0; i < m_objectList.size(); i++)
	{
		if (m_objectList[i]->isPhysical & (!m_objectList[i]->isScrolingObject || m_objectList[i]->name == "Skeleton"))
			UpdatePhysics(i, deltaT);
	}

	//COLLISIONS
	
	//sort list
	std::sort(m_objectList.begin(), m_objectList.end(), wayToSort);

	//Check For Collisions
	for (int i = 0; i < m_objectList.size(); i++)
	{

			//check collsions with all objects to the right of each object
			for (int k = i + 1; k < m_objectList.size(); k++)
			{
				if ((m_objectList[i]->isCollidable || m_objectList[k]->isCollidable) && (!m_objectList[i]->isScrolingObject || !m_objectList[k]->isScrolingObject))
				{

						//check horizontal distance between
						float distX = m_objectList[k]->pos.x - m_objectList[i]->pos.x;
						//if within range, check hight range
						if (distX < maxCollisionDistance)
						{
							float distY = m_objectList[k]->pos.y - m_objectList[i]->pos.y;
							float dist = sqrt((distX * distX) + (distY * distY));
							if (dist < (m_objectList[i]->radius + m_objectList[k]->radius))
							{
								if ((m_objectList[i]->name == "Skeleton" || m_objectList[k]->name == "Skeleton"))
									break;
								if (m_objectList[i]->isPhysical && m_objectList[k]->isPhysical)
								{
									ResolveCollision(m_objectList[i], m_objectList[k]);
								}

								m_objectList[i]->CollidedWith(m_objectList[k]);
								m_objectList[k]->CollidedWith(m_objectList[i]);
							}
						}
						else
							break;
				}


			//if this ball has hit a boundry wall
			if (!m_objectList[i]->isScrolingObject || m_objectList[i]->name == "Skeleton")
			{
				if (m_objectList[i]->pos.y > 720 - m_objectList[i]->radius)
				{
					float error = m_objectList[i]->pos.y - (720 - m_objectList[i]->radius);
					m_objectList[i]->pos.y = (720 - m_objectList[i]->radius) - error;
					m_objectList[i]->vel.y = -m_objectList[i]->vel.y;			//coefficient of restitution refers to how much of the force is absorbed during the collision for deformation etc
				}
				if (m_objectList[i]->pos.y < m_objectList[i]->radius)
				{
					float error = m_objectList[i]->pos.y - m_objectList[i]->radius;
					m_objectList[i]->pos.y = m_objectList[i]->radius - error;
					m_objectList[i]->vel.y = -m_objectList[i]->vel.y;			//coefficient of restitution refers to how much of the force is absorbed during the collision for deformation etc
				}
				if (m_objectList[i]->pos.x > 1280 - m_objectList[i]->radius && !m_objectList[i]->isScrolingObject)
				{
					float error = m_objectList[i]->pos.x - (1280 - m_objectList[i]->radius);
					m_objectList[i]->pos.x = (1280 - m_objectList[i]->radius) - error;
					m_objectList[i]->vel.x = -m_objectList[i]->vel.x;			//coefficient of restitution refers to how much of the force is absorbed during the collision for deformation etc
				}
				if (m_objectList[i]->pos.x < m_objectList[i]->radius && !m_objectList[i]->isScrolingObject)
				{
					float error = m_objectList[i]->pos.x - m_objectList[i]->radius;
					m_objectList[i]->pos.x = m_objectList[i]->radius - error;
					m_objectList[i]->vel.x = -m_objectList[i]->vel.x;			//coefficient of restitution refers to how much of the force is absorbed during the collision for deformation etc
				}
			}
		}
	}

	// Clean up empty items from our list:
	m_objectList.erase(
		std::remove(m_objectList.begin(), m_objectList.end(), (Object*)NULL),
		m_objectList.end()
		);
}

void Factory :: UpdatePhysics(int index, float deltaT)				
{
	//sum forces:
	m_objectList[index]->acc.y = 50.0; //gravity

	//integrate
	//dv = a * dt  - delta velocity = acceleration * delta time
	sf::Vector2f dv = m_objectList[index]->acc * deltaT;
	m_objectList[index]->vel += dv;

	//dp = v * dt
	sf::Vector2f dp = m_objectList[index]->vel * deltaT;
	m_objectList[index]->pos += dp;

	//DRAG
	m_objectList[index]->vel = m_objectList[index]->vel * powf(0.45f, deltaT);
}

void Factory::ResolveCollision(Object* a, Object* b)
{
	Vector2f vectorFromBToA = a->pos - b->pos;
	if (vectorFromBToA == Vector2f(0, 0))
		std::cout << "ERROR: Attempting to resolve collisions between two objects at the same physical point!" << std::endl;

	//push objects apart
	Vector2f midPoint = (b->pos + a->pos) * 0.5f;
	Vector2f normalFromBtoA = Normslised(vectorFromBToA);

	if (!a->isScrolingObject && !b->isScrolingObject && a->name != "Skeleton" && b-> name != "Skeleton")
	{
		a->pos = midPoint + normalFromBtoA * a->radius;
		b->pos = midPoint - normalFromBtoA * b->radius;
	}
	else
	{
		if (b->isScrolingObject)
			a->pos = b->pos + normalFromBtoA * (a->radius + b->radius);
		else if (a->isScrolingObject)
			b->pos = a->pos - normalFromBtoA * (a->radius + b->radius);
	}
		

	//seperate velocities
	Vector2f a1 = DotProduct(normalFromBtoA, b->vel) * normalFromBtoA;		// obj 1's vel in dir of collision
	Vector2f b1 = b->vel - a1;												// obj 1's vel perpendicular to  collision

	Vector2f a0 = DotProduct(-normalFromBtoA, a->vel) * -normalFromBtoA;	// obj 0's vel in dir of collision
	Vector2f b0 = a->vel - a0;												// obj 0's vel perpendicular to  collision

	//swap collision velocities
	std::swap(a1, a0);

	a->vel = a0 + b0;
	b->vel = a1 + b1;

	if (a->isScrolingObject)
		a->vel = sf::Vector2f(0, 0);
	if (b->isScrolingObject)
		b->vel = sf::Vector2f(0, 0);
}